#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import argparse

# import numpy as np
import sys

import serial  # 导入模块
import serial.tools.list_ports
import threading
import struct
import time
import platform
import numpy as np
# from copy import deepcopy
# import sys
# import os
# import math

#from serial import EIGHTBITS, PARITY_NONE, STOPBITS_ONE
import ecal.core.core as ecal_core

from ecal.core.publisher import StringPublisher
import imu_proto_messages.imu_proto_pb2 as imu_proto_pb2
from ecal.core.publisher import ProtoPublisher

# 宏定义参数
PI = 3.1415926
FRAME_HEAD = str('fc')
FRAME_END = str('fd')
TYPE_IMU = str('40')
TYPE_AHRS = str('41')
TYPE_INSGPS = str('42')
TYPE_GEODETIC_POS = str('5c')
TYPE_GROUND = str('f0')
TYPE_SYS_STATE = str('50')
IMU_LEN = str('38')  # //56
AHRS_LEN = str('30')  # //48
INSGPS_LEN = str('48')  # //80
GEODETIC_POS_LEN = str('20')  # //32
SYS_STATE_LEN = str('64')  # // 64
PI = 3.141592653589793
DEG_TO_RAD = 0.017453292519943295
isrun = True

# 获取命令行输入参数
def parse_opt(known=False):
    parser = argparse.ArgumentParser()
    # parser.add_argument('--debugs', type=bool, default=False, help='if debug info output in terminal ')
    parser.add_argument('--port', type=str, default='/dev/ttyUSB1', help='the models serial port receive data; example: '
                                                                 '    Windows: COM8'
                                                                 '    Linux: /dev/ttyUSB0')

    parser.add_argument('--bps', type=int, default=460800, help='the models baud rate set; default: 460800')
    parser.add_argument('--timeout', type=int, default=20, help='set the serial port timeout; default: 20')
    # parser.add_argument('--device_type', type=int, default=0, help='0: origin_data, 1: for single imu or ucar in ROS')

    receive_params = parser.parse_known_args()[0] if known else parser.parse_args()
    return receive_params


# 接收数据线程
def receive_data():
    open_port()
    # 尝试打开串口
    try:
        serial_ = serial.Serial(port=opt.port, baudrate=opt.bps, bytesize=serial.EIGHTBITS, parity=serial.PARITY_NONE,
                                stopbits=serial.STOPBITS_ONE,
                                timeout=opt.timeout)
        print("baud rates = " + str(serial_.baudrate))
    except:
        print("error:  unable to open port .")
        exit(1)
    
    ecal_core.initialize(sys.argv, "Imu Protobuf Publisher")

    ecalcontrolMovepublisher = ProtoPublisher("ImuMessage_topic", imu_proto_pb2.controlImu)
    
    # 循环读取数据
    while serial_.isOpen() and tr.is_alive() :
        # rbdata = ser.readline()
        # # rbdata = ser.read_all()
        #
        # if len(rbdata) != 0:
        #     rxdata = rbdata.hex()
        #     print(rxdata)
        if not threading.main_thread().is_alive():
            print('done')
            break
        check_head = serial_.read().hex()
        # 校验帧头
        print(check_head,end='')
        if check_head != FRAME_HEAD:
            continue
        head_type = serial_.read().hex()
        # 校验数据类型
        if (head_type != TYPE_IMU and head_type != TYPE_AHRS and head_type != TYPE_INSGPS and
                head_type != TYPE_GEODETIC_POS and head_type != 0x50 and head_type != TYPE_GROUND and
                head_type != TYPE_SYS_STATE):
            continue
        check_len = serial_.read().hex()
        # 校验数据类型的长度
        if head_type == TYPE_IMU and check_len != IMU_LEN:
            continue
        elif head_type == TYPE_AHRS and check_len != AHRS_LEN:
            continue
        elif head_type == TYPE_INSGPS and check_len != INSGPS_LEN:
            continue
        elif head_type == TYPE_GEODETIC_POS and check_len != GEODETIC_POS_LEN:
            continue
        elif head_type == TYPE_SYS_STATE and check_len != SYS_STATE_LEN:
            continue
        elif head_type == TYPE_GROUND or head_type == 0x50:
            continue
        check_sn = serial_.read().hex()
        head_crc8 = serial_.read().hex()
        crc16_H_s = serial_.read().hex()
        crc16_L_s = serial_.read().hex()
        
        
        protobuf_message =imu_proto_pb2.controlImu()
        
        # 读取并解析IMU数据
        if head_type == TYPE_IMU:
            data_s = serial_.read(int(IMU_LEN, 16))
            '''print("Gyroscope_X(rad/s): " + str(struct.unpack('f', data_s[0:4])[0]))
            print("Gyroscope_Y(rad/s) : " + str(struct.unpack('f', data_s[4:8])[0]))
            print("Gyroscope_Z(rad/s) : " + str(struct.unpack('f', data_s[8:12])[0]))
            print("Accelerometer_X(m/s^2) : " + str(struct.unpack('f', data_s[12:16])[0]))
            print("Accelerometer_Y(m/s^2) : " + str(struct.unpack('f', data_s[16:20])[0]))
            print("Accelerometer_Z(m/s^2) : " + str(struct.unpack('f', data_s[20:24])[0]))'''
            # print("Magnetometer_X(mG) : " + str(struct.unpack('f', data_s[24:28])[0]))
            # print("Magnetometer_Y(mG) : " + str(struct.unpack('f', data_s[28:32])[0]))
            # print("Magnetometer_Z(mG) : " + str(struct.unpack('f', data_s[32:36])[0]))
            # print("IMU_Temperature : " + str(struct.unpack('f', data_s[36:40])[0]))
            # print("Pressure : " + str(struct.unpack('f', data_s[40:44])[0]))
            # print("Pressure_Temperature : " + str(struct.unpack('f', data_s[44:48])[0]))
            # print("Timestamp(us) : " + str(struct.unpack('ii', data_s[48:56])[0]))
        # 读取并解析AHRS数据
        elif head_type == TYPE_AHRS:
            data_s = serial_.read(int(AHRS_LEN, 16))
            #print("RollSpeed(rad/s): " + str(struct.unpack('f', data_s[0:4])[0]))
            #print("PitchSpeed(rad/s) : " + str(struct.unpack('f', data_s[4:8])[0]))
            #print("HeadingSpeed(rad/s) : " + str(struct.unpack('f', data_s[8:12])[0]))
            '''print("Roll(rad) : " + str(struct.unpack('f', data_s[12:16])[0]))
            print("Pitch(rad) : " + str(struct.unpack('f', data_s[16:20])[0]))
            print("Heading(rad) : " + str(struct.unpack('f', data_s[20:24])[0]))
            print("Q1 : " + str(struct.unpack('f', data_s[24:28])[0]))
            print("Q2 : " + str(struct.unpack('f', data_s[28:32])[0]))
            print("Q3 : " + str(struct.unpack('f', data_s[32:36])[0]))
            print("Q4 : " + str(struct.unpack('f', data_s[36:40])[0]))'''
            Q1=struct.unpack('f', data_s[24:28])[0]
            Q2=struct.unpack('f', data_s[28:32])[0]
            Q3=struct.unpack('f', data_s[32:36])[0]
            Q4=struct.unpack('f', data_s[36:40])[0]
            ##print('Q1,Q2,Q3,Q4',Q1,Q2,Q3,Q4)
            
            
            Roll_rad=struct.unpack('f', data_s[12:16])[0]
            Pitch_rad=struct.unpack('f', data_s[16:20])[0]
            Heading_rad=2*PI-struct.unpack('f', data_s[20:24])[0]
            print('Roll_rad,Pitch_rad,Heading_rad', np.degrees(Roll_rad), np.degrees(Pitch_rad), np.degrees(Heading_rad))
            RollSpeed = struct.unpack('f', data_s[0:4])[0]
            PitchSpeed = struct.unpack('f', data_s[16:20])[0]
            HeadingSpeed = struct.unpack('f', data_s[8:12])[0]*0#need to test again
            
            protobuf_message.Roll_rad= Roll_rad
            protobuf_message.Pitch_rad= Pitch_rad
            protobuf_message.Heading_rad= Heading_rad

            protobuf_message.RollSpeed = RollSpeed
            protobuf_message.PitchSpeed = PitchSpeed
            protobuf_message.HeadingSpeed = HeadingSpeed
            
        
            ecalcontrolMovepublisher.send(protobuf_message)
            
            #print('roll_deg,pitch_deg,yaw_deg',roll_deg,pitch_deg,yaw_deg)
            
            # print("Timestamp(us) : " + str(struct.unpack('ii', data_s[40:48])[0]))
        # 读取并解析INSGPS数据
        elif head_type == TYPE_INSGPS:
            data_s = serial_.read(int(INSGPS_LEN, 16))
            print("BodyVelocity_X:(m/s)" + str(struct.unpack('f', data_s[0:4])[0]))
            print("BodyVelocity_Y:(m/s)" + str(struct.unpack('f', data_s[4:8])[0]))
            print("BodyVelocity_Z:(m/s)" + str(struct.unpack('f', data_s[8:12])[0]))
            print("BodyAcceleration_X:(m/s^2)" + str(struct.unpack('f', data_s[12:16])[0]))
            print("BodyAcceleration_Y:(m/s^2)" + str(struct.unpack('f', data_s[16:20])[0]))
            print("BodyAcceleration_Z:(m/s^2)" + str(struct.unpack('f', data_s[20:24])[0]))
            print("Location_North:(m)" + str(struct.unpack('f', data_s[24:28])[0]))
            print("Location_East:(m)" + str(struct.unpack('f', data_s[28:32])[0]))
            print("Location_Down:(m)" + str(struct.unpack('f', data_s[32:36])[0]))
            print("Velocity_North:(m)" + str(struct.unpack('f', data_s[36:40])[0]))
            print("Velocity_East:(m/s)" + str(struct.unpack('f', data_s[40:44])[0]))
            print("Velocity_Down:(m/s)" + str(struct.unpack('f', data_s[44:48])[0]))
            # print("Acceleration_North:(m/s^2)" + str(struct.unpack('f', data_s[48:52])[0]))
            # print("Acceleration_East:(m/s^2)" + str(struct.unpack('f', data_s[52:56])[0]))
            # print("Acceleration_Down:(m/s^2)" + str(struct.unpack('f', data_s[56:60])[0]))
            # print("Pressure_Altitude:(m)" + str(struct.unpack('f', data_s[60:64])[0]))
            # print("Timestamp:(us)" + str(struct.unpack('ii', data_s[64:72])[0]))
        # 读取并解析GPS数据
        elif head_type == TYPE_GEODETIC_POS:
            data_s = serial_.read(int(GEODETIC_POS_LEN, 16))
            print(" Latitude:(rad)" + str(struct.unpack('d', data_s[0:8])[0]))
            print("Longitude:(rad)" + str(struct.unpack('d', data_s[8:16])[0]))
            print("Height:(m)" + str(struct.unpack('d', data_s[16:24])[0]))
        elif head_type == TYPE_SYS_STATE:
            data_s = serial_.read(int(SYS_STATE_LEN, 16))
            print(" System_status:" + str(struct.unpack('d', data_s[0:2])[0]))


# 寻找输入的port串口
def find_serial():
    port_list = list(serial.tools.list_ports.comports())
    for port in port_list:
        if port.device == opt.port:
            return True
    return False


def open_port():
    if find_serial():
        print("find this port : " + opt.port)
    else:
        print("error:  unable to find this port : " + opt.port)
        exit(1)


def UsePlatform():
    sys_str = platform.system()
    if sys_str == "Windows":
        print("Call Windows tasks")
    elif sys_str == "Linux":
        print("Call Linux tasks")
    else:
        print("Other System tasks: %s" % sys_str)
    return sys_str


if __name__ == "__main__":
    print(UsePlatform())
    opt = parse_opt()
    tr = threading.Thread(target=receive_data)
    tr.start()
    while True:
        try:
            if tr.is_alive():
                time.sleep(1)
            else:
                break
        except(KeyboardInterrupt, SystemExit):
            break
